#pragma once
#include <Eigen/Dense>
#include <vector>
class align_ENU
{
private:
    Eigen::Quaterniond q;
    Eigen::Vector3d t;
    struct PathPointType
    {
        Eigen::Vector3d point;
        double time_stamp;
        /// @brief 0 gps 1 lidar
        int point_type;
    };
    int origin_point_size;
    int align_to_point_size;
    std::vector<PathPointType> point_seq; 

    
public:
    align_ENU(/* args */);
    ~align_ENU();
    void add_origin_path_point(Eigen::Vector3d pose,double time_stamp);
    void add_align_to_path_point(Eigen::Vector3d pose,double time_stamp);
    void get_result(Eigen::Quaterniond &q_,Eigen::Vector3d &t_);
    bool optimization(double &score);
    void set_init_qt(Eigen::Quaterniond q,Eigen::Vector3d t);
    int get_origin_point_size();
    int get_align_to_point_size();
    PathPointType & back(){
        return point_seq.back();
    }
    PathPointType & front(){
        return point_seq.front();
    }
    size_t size(){
        return point_seq.size();
    }
};

